#!/home/lin/software/miniconda3/envs/th1.11/bin/python
# -*- coding: utf-8 -*-
'''
体素降采样通过使用规则体素网格从输入点云创造一致化降采样点云
把点云装进体素网格
把每个被占据的体素中的点做平均，取一个精确的点。
'''

import open3d as o3d
import numpy as np

# -----------1. 读取------------
print("Downsample the point cloud with a voxel of 0.05")
pcd = o3d.io.read_point_cloud("../data/cloud_bin_0.pcd")

# 保存
# o3d.io.write_point_cloud("copy_of_fragment.pcd", pcd)
# -----------2. 体素滤波降采样-----------
downpcd = pcd.voxel_down_sample(voxel_size=0.05)# 使用0.05m的体素对点云进行下采样

# 使用N键显示发现并用+ - 调节法线长度
o3d.visualization.draw_geometries([downpcd])

# ------------3. 估计法线----------------
print("Recompute the normal of the downsampled point cloud")
# estimate_normals 计算每个点的法线。该函数查找相邻点，并使用协方差分析计算相邻点的主轴。
'''
法线 椭球的短轴
KDTreeSearchParamHybridclass的实例作为参数。这两个关键参数，并指定搜索近邻半径和最大。
它的搜索半径为10厘米，最多可考虑30个邻居，以节省计算时间。
radius = 0.1.  max_nn = 30  
'''
downpcd.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=0.1, max_nn=30))

print(downpcd.normals[0]) #打印第0个点的法线向量
o3d.visualization.draw_geometries([downpcd], point_show_normal=True)


# ----------4. 凸包---------------
# 点云的凸包是包含所有点的最小凸集
hull, _ = pcd.compute_convex_hull()
hull_ls = o3d.geometry.LineSet.create_from_triangle_mesh(hull)
hull_ls.paint_uniform_color((1, 0, 0))
# o3d.visualization.draw_geometries([pcd, hull_ls])

aabb = pcd.get_axis_aligned_bounding_box() #返回几何的轴对齐边界框
aabb.color = (1, 0, 0)
obb = pcd.get_oriented_bounding_box() #返回几何体的定向边界框
obb.color = (0, 1, 0)
o3d.visualization.draw_geometries([pcd, aabb, obb, hull_ls])